package com.metaio.Template;

import com.metaio.sdk.jni.ETRACKING_STATE;
import com.metaio.sdk.jni.Rotation;
import com.metaio.sdk.jni.SensorValues;
import com.metaio.sdk.jni.TrackingValues;
import com.metaio.sdk.jni.Vector3d;
import com.metaio.sdk.jni.Vector4d;

/**
 * This class help us to transit from a map to the next one
 * without the user noticing the transition
 * @author chiara
 *
 */
public class MapTransitionHelper {
	
	//storing the last pose and orientation to update the pose while a new map is initializing
	private boolean lastCameraPoseValid=false;
	
	//cos offset needed to stay in the same cos between to different maps
	private TrackingValues offsetNewCOSFromOldCOS=new TrackingValues();
	
	private boolean offsetRecomputedAfterLastInitialization=false;
	
	private TrackingValues lastCameraPose=new TrackingValues();
	private SensorValues lastSensorValues=new SensorValues();
	
	public MapTransitionHelper(){
		reset();
	}

	public void reset() {
		offsetNewCOSFromOldCOS.setRotation(new Rotation(new Vector4d(0, 0, 0, 0)));
		offsetNewCOSFromOldCOS.setTranslation(new Vector3d(0, 0, 0));
		
		lastCameraPose.setTranslation(new Vector3d(0, 0, 0));
		lastCameraPose.setRotation(new Rotation(new Vector4d(0, 0, 0, 0)));
		
		lastCameraPoseValid=false;
		offsetRecomputedAfterLastInitialization=false;
	}

	/**
	 * call this method when a transition to a new map is expected soon
	 */
	public void prepareForTransitionToNewMap(){
		offsetRecomputedAfterLastInitialization=true;
	}
	
	/**
	 * update the internal state with the latest tracking value from the SDK
	 * @param trackingValues
	 * @param sensorValues
	 */
	public void update(TrackingValues trackingValues, SensorValues sensorValues) {
		
		//agenda 1: if the tracking is not lost -> update the internal state
		//		 2: if the tracking is lost -> use the last tracked value until SLAM is reinitialized
		
				
		if (trackingValues.getQuality() > 0 & trackingValues.getState()!=ETRACKING_STATE.ETS_INITIALIZATION_FAILED) {
			
			//special case in which a new map was started so we have to update the offset
			if (offsetRecomputedAfterLastInitialization & lastCameraPoseValid) {

				offsetNewCOSFromOldCOS.setRotation(trackingValues.getRotation()
						.inverse().multiply(lastCameraPose.getRotation()));

				offsetNewCOSFromOldCOS.setTranslation(trackingValues
						.getRotation().inverse()
						.rotatePoint(lastCameraPose.getTranslation())
						.add(trackingValues.getInverseTranslation()));

			}

			//frame can be tracked -> recompute and update the last camera pose
			TrackingValues globalCameraPose = trackingValues;
			globalCameraPose.setRotation(trackingValues.getRotation().multiply(
					offsetNewCOSFromOldCOS.getRotation()));
			globalCameraPose.setTranslation(trackingValues.getTranslation()
					.add(trackingValues.getRotation().rotatePoint(
							offsetNewCOSFromOldCOS.getTranslation())));
			
			lastCameraPose=globalCameraPose;
			lastCameraPoseValid=true;
		}else{
			
			//tracking is lost, we have to extrapolate the orientation
			if(lastCameraPoseValid){
				
				//we can use the info on the lost tracking to bridging the current initialization attempt
				SensorValues sv=sensorValues;
				
				//compute the relative rotation
				Rotation currentFromLastFrame=sv.getAttitude().multiply(lastSensorValues.getAttitude().inverse());
				
				//apply it to the pose
				Rotation currentCameraPoseR=currentFromLastFrame.multiply(lastCameraPose.getRotation());
				Vector3d currentCameraPoseT=currentFromLastFrame.rotatePoint(lastCameraPose.getTranslation());
				
				//save this to the parameter that hold the last pose
				lastCameraPose.setTranslation(currentCameraPoseT);
				lastCameraPose.setRotation(currentCameraPoseR);
			}
		}
		
		lastSensorValues=sensorValues;
	}
	
	/**
	 * @return rotation of the last camera pose
	 */
	public Rotation getRotationCameraFromWorld() {
		return lastCameraPose.getRotation();
	}
	
	/**	
	 * @return translation of the last camera pose
	 */
	public Vector3d getTranslationCameraFromWorld(){
		return lastCameraPose.getTranslation();
	}
	
	/**
	 * @return true if the last frame was tracked
	 */
	public boolean lastFrameWasTracked(){
		return (lastCameraPose.getQuality()>0&lastCameraPose.isTrackingState());
	}
}
